package simulator.implementation.agent;
import java.awt.image.BufferedImage;

import javax.vecmath.Vector3d;

import ai.basic.AIController;
import ai.basic.Action;
import ai.basic.Fact;
import ai.basic.Pattern;
import ai.knn.AgentBrain;

import simbad.sim.CameraSensor;
import simbad.sim.RangeSensorBelt;
import simbad.sim.RobotFactory;
import simulator.meta.agent.ControlledAgent;
public class IntelligentRobot extends ControlledAgent{
	//Constants:
	private final byte NUMBER_OF_ULTRASONIC_SENSORS = 10;
	//Attributes:
	private boolean thereIsCommand;
	private CameraSensor camera;
	private BufferedImage imageFromCamera;
	private RangeSensorBelt sonars;
	private AIController agentBrain;
	//Constructor: 
	public IntelligentRobot(Vector3d position, String name) {
		super(position, name);
		this.step = 0;
		this.turn = 0;
		this.thereIsCommand = false;
		this.camera = RobotFactory.addCameraSensor(this);
		this.imageFromCamera = this.camera.createCompatibleImage();
		this.sonars = RobotFactory.addSonarBeltSensor(this, this.NUMBER_OF_ULTRASONIC_SENSORS);
		//this.agentBrain = new AgentBrain(name,AgentBrain.NOT_REPLACE_WHEN_NEW_PATTERN);
		this.agentBrain = new AgentBrain(name,AgentBrain.REPLACE_WHEN_NEW_PATTERN);
		//this.agentBrain = new AgentBrain(name,AgentBrain.EXCECUTE);
		//this.agentBrain.setOperationMode(AgentBrain.OPERATE_WITH_EUCLIDEAN_DISTANCE);
		this.agentBrain.setOperationMode(AgentBrain.OPERATE_WITH_SQUARE_ERROR);
	}
	//Methods:
	public void initBehavior() {}

	public void performBehavior() {
		double[]actionValue = {this.step,(Math.PI/180)*this.turn};
		if((this.getCounter()%2)==0){
			double measure; boolean hit;
			short[]patternValue = new short[this.sonars.getNumSensors()];
			System.out.println("=========================");
			for(int i = 0;i<this.sonars.getNumSensors();i++){
				measure = this.sonars.getMeasurement(i);
				hit = this.sonars.hasHit(i);
				if(!hit){
					measure = 1.4;
				}
				System.out.println("Info: ("+i+") = "+measure+", "+hit);
				patternValue[i] = (short)(measure*100);
			}
			Action action;
			Pattern pattern = new Pattern();
			pattern.setValue(patternValue);
			if(this.thereIsCommand){
				System.out.println("Commanding...");
				Fact fact = new Fact();
				action = new Action();
				action.setValue(actionValue);
				fact.setPattern(pattern);
				fact.setAction(action);
				System.out.println("\t* Entering pattern: "+this.agentBrain.enterPatter(fact)+".");
				this.thereIsCommand = false;
			}else{
				System.out.println("Quering...");
				action = this.agentBrain.query(pattern);
				if(action!=null){
					actionValue = action.getValue();
					System.out.println("\t* Action 0K!");
				}else{
					System.out.println("\t* No action!");
				}
			}
		}
		this.setTranslationalVelocity(actionValue[0]);
		this.setRotationalVelocity(actionValue[1]);
		// get camera image
		camera.copyVisionImage(this.imageFromCamera);
	}
}
